/*
Copyright (C) 2008, 2009 Henrik Naess

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 3 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library. If not, see <http://www.gnu.org/licenses/>.
*/

using System;
using System.Collections.Generic;
using ZoneFiveSoftware.Common.Data;
using ZoneFiveSoftware.Common.Data.Fitness;
using ZoneFiveSoftware.Common.Data.GPS;

namespace MatrixPlugin
{
    public class MatrixPoint : ExtendedGPSPoint
    {
        private Dictionary<string, IList<ClosestEntryStruct>> m_closestEntries;
        private float m_maxDistanceError = (float)Settings.MaxRadius;
        private float m_currentPointDiffFactor = (float)Settings.CurrentPointDiffFactor;
        private ClosestEntryStruct m_currClose = new ClosestEntryStruct(null, null, float.MaxValue);

        /// <summary>
        /// Store information to find closest tracked point to the GPSPoint's point
        /// </summary>
        internal struct ClosestEntryStruct
        {
            internal ClosestEntryStruct(IActivity activity, ITimeValueEntry<IGPSPoint> entry, float closestDistanceError)
            {
                this.activity = activity;
                this.entry = entry;
                distanceError = closestDistanceError;
                distanceFromStart = float.MaxValue;
                secondsFromStart = float.MaxValue;
                startTime = DateTime.MinValue;
                stoppedTime = new TimeSpan(0);
            }

            // Working data for position
            internal IActivity activity;
            internal ITimeValueEntry<IGPSPoint> entry;
            internal float distanceError;
            internal float distanceFromStart;
            internal float secondsFromStart;
            internal DateTime startTime;
            internal TimeSpan stoppedTime;
        }

        #region Constructor

        /// <summary>
        /// Constructor for creating a Point
        /// </summary>
        /// <param name="name">name of the point</param>
        /// <param name="gpsPoint">point</param>
        internal MatrixPoint(string name, GPSPoint gpsPoint) 
            : base(name, gpsPoint)
        {
            InitClassVariables();
        }

        /// <summary>
        /// Constructor for creating a Point
        /// </summary>
        /// <param name="name">name of the point</param>
        /// <param name="latitude">latitude for gps point</param>
        /// <param name="longitude">longitude for gps point</param>
        internal MatrixPoint(string name, float latitude, float longitude)
            : base(name, latitude, longitude)
        {
            InitClassVariables();
        }
        internal MatrixPoint(string name, float latitude, float longitude, float radius)
            : base(name, latitude, longitude)
        {
            InitClassVariables(radius);
        }

        private void InitClassVariables(float maxDistanceError)
        {
            m_closestEntries = new Dictionary<string, IList<ClosestEntryStruct>>();
            m_maxDistanceError = maxDistanceError;
        }
        private void InitClassVariables()
        {
            InitClassVariables((float)Settings.MaxRadius);
        }

        #endregion

        /// <summary>
        /// Found a GPSPoint for the point (ProcessEntry has been run, and I match must have been found)
        /// </summary>
        /// <returns>Found a GPSPoint</returns>
        internal bool Found
        {
            get
            {
                //bool found = false;
                foreach (KeyValuePair<string, IList<ClosestEntryStruct>> entry in m_closestEntries)
                {
                    if (entry.Value != null)
                    {
                        foreach (ClosestEntryStruct ces in entry.Value)
                        {
                            if (ces.distanceError < float.MaxValue)
                            {
                                return true;
                            }
                        }
                    }
                }
                return false;
            }
        }

        /// <summary>
        /// Clean current information on closest GPSPoint to (class') point, and other relevant variables
        /// </summary>
        internal void ClearCalculations()
        {
            m_closestEntries.Clear();
        }

        public DistanceInfoList GetDistanceData(MatrixPoint otherPoint, bool includeStoppedTime)
        {
            DistanceInfoList distanceInfoList = new DistanceInfoList();
            foreach (KeyValuePair<string, IList<ClosestEntryStruct>> thisCloseActivities in this.m_closestEntries)
            {
                IList<ClosestEntryStruct> thisCloseList = thisCloseActivities.Value;
                if (thisCloseList != null)
                {
                    foreach (ClosestEntryStruct thisClosestEntry in thisCloseList)
                    {
                        IList < ClosestEntryStruct > otherCloseList;
                        if (otherPoint.m_closestEntries.TryGetValue(thisCloseActivities.Key, out otherCloseList))
                        //foreach (KeyValuePair<string, IList<ClosestEntryStruct>> otherCloseActivities in otherPoint._closestEntries)
                        {
                            if (otherCloseList != null)
                            {
                                foreach (ClosestEntryStruct otherClosestEntry in otherCloseList)
                                {
                                    // Should now have to closest entries, for the same activity
                                    if (thisClosestEntry.activity == otherClosestEntry.activity &&
                                        !thisClosestEntry.Equals(otherClosestEntry) &&
                                        otherClosestEntry.distanceFromStart < float.MaxValue &&
                                        otherClosestEntry.secondsFromStart > thisClosestEntry.secondsFromStart)
                                    {
                                        // Calculate data for distance info object
                                        float distance = otherClosestEntry.distanceFromStart - thisClosestEntry.distanceFromStart;
                                        float distanceTime = otherClosestEntry.secondsFromStart - thisClosestEntry.secondsFromStart;
                                        TimeSpan stoppedTime = otherClosestEntry.stoppedTime - thisClosestEntry.stoppedTime;
                                        TimeSpan ts = new TimeSpan(0, 0, (int)distanceTime);
                                        DateTime startTime = thisClosestEntry.startTime;
                                        // both this and other should have same start time, since same activity

                                        // Create DistanceInfo object and add to list
                                        DistanceInfo distanceInfo = new DistanceInfo(thisClosestEntry.activity, startTime, ts, distance, stoppedTime,
                                                                                     includeStoppedTime);
                                        distanceInfo.SetElevation(otherClosestEntry.entry.Value.ElevationMeters,
                                                                  thisClosestEntry.entry.Value.ElevationMeters);
                                        distanceInfoList.Add(distanceInfo);
                                    }
                                    //else
                                    //   Debug.WriteLine(string.Format("For activity {0} {1} ({2}s.)is before {3} ({4}s.)!",
                                    //                                 activity.Key, otherPoint.Name, thisClosestEntry.secondsFromStart, Name, otherClosestEntry.secondsFromStart));
                                }
                            }
                        }
                    }
                }
            }
            return distanceInfoList;
        }

        //Check for points that "pass" the radius 
        //Taken from Trails plugin, ActivityTrail.cs
        private float checkPass(float radius, IGPSPoint r1, float dt1, IGPSPoint r2, float dt2, IGPSPoint trailp)
        {
            double d;
            return checkPass(radius, r1, dt1, r2, dt2, trailp, out d);
        }
        private float checkPass(float radius, IGPSPoint r1, float dt1, IGPSPoint r2, float dt2, IGPSPoint trailp, out double d)
        {
            d = double.MaxValue;
            float factor = -1;
            if (r1 == null || r2 == null || trailp == null) return factor;

            //Check if the line goes via the "circle" if the sign changes
            //Also need to check close points that fit in a 45 deg tilted "square" where sign may not change
            if (r1.LatitudeDegrees > trailp.LatitudeDegrees
                                && r2.LatitudeDegrees < trailp.LatitudeDegrees
                || r1.LatitudeDegrees < trailp.LatitudeDegrees
                                && r2.LatitudeDegrees > trailp.LatitudeDegrees
                || r1.LongitudeDegrees > trailp.LongitudeDegrees
                                && r2.LongitudeDegrees < trailp.LongitudeDegrees
                || r1.LongitudeDegrees < trailp.LongitudeDegrees
                                && r2.LongitudeDegrees > trailp.LongitudeDegrees
                || dt1 < radius * Math.Sqrt(2)
                && dt2 < radius * Math.Sqrt(2))
            {
                //Law of cosines - get a1, angle at r1, the first point
                double d12 = r1.DistanceMetersToPoint(r2);
                double a10 = (dt1 * dt1 + d12 * d12 - dt2 * dt2) / (2 * dt1 * d12);
                //Point is in circle if closest point is between r1&r2 and it is in circle (neither r1 nor r2 is)
                //This means the angle a1 must be +/- 90 degrees : cos(a1)>=0
                if (a10 > -0.001)
                {
                    //Rounding errors w GPS measurements
                    a10 = Math.Min(a10, 1);
                    a10 = Math.Max(a10, -1);
                    double a1 = Math.Acos(a10);
                    //Dist from r1 to closest point on d1-d2 (called x)
                    double d1x = Math.Abs(dt1 * Math.Cos(a1));
                    //Dist from t1 to x on line between d1-d2
                    double dtx = dt1 * Math.Sin(a1);
                    if (d1x < d12 && dtx < radius)
                    {
                        d = dtx;
                        factor = (float)(d1x / d12);
                        //Return factor, to return best aproximation from r1
                    }
                }
            }
            return factor;
        }

        public void CheckForNewClosestEntry(IActivity activity, ITimeValueEntry<IGPSPoint> newEntry, GPSRoute tempRoute,
                                            TimeSpan stoppedTime, ITimeValueEntry<IGPSPoint> prevPoint)
        {
            if (null == activity)
                throw new ApplicationException("Activity is NULL!");

            float difference = newEntry.Value.DistanceMetersToPoint(m_gpsPoint);
            bool isMatch = false;

            if (difference <= m_maxDistanceError)
            {
                isMatch = true;
            }
            else if (null != prevPoint)
            {
                //prune calculations, a better already exists
                if (!activity.Equals(m_currClose.activity) || difference < m_currClose.distanceError)
                {
                    float prevDistToPoint = prevPoint.Value.DistanceMetersToPoint(m_gpsPoint);
                    float factor = checkPass(m_maxDistanceError, prevPoint.Value, prevDistToPoint, newEntry.Value,
                        difference, m_gpsPoint);
                    if (0 < factor)
                    {
                        isMatch = true;
                        //An estimated point (including time) could be inserted in the track and used
                        //Just add the latest point, even if it is not the best
                    }
                }
            }
            if (isMatch && (m_currClose.activity == null || 
                activity.Equals(m_currClose.activity) && difference < m_currClose.distanceError))
            {
                //This match is "better" than the possible current
                IList<ClosestEntryStruct> closestList;
                if (!m_closestEntries.TryGetValue(activity.ReferenceId, out closestList))
                {
                    closestList = new List<ClosestEntryStruct>();
                    m_closestEntries.Add(activity.ReferenceId, closestList);
                }
                //Remove current closest
                if (m_currClose.activity == activity &&
                    closestList.Count > 0)
                {
                    closestList.RemoveAt(closestList.Count - 1);
                }
                m_currClose.activity = activity;
                m_currClose.entry = newEntry;
                m_currClose.distanceError = difference;
                m_currClose.distanceFromStart = tempRoute.TotalDistanceMeters;
                m_currClose.secondsFromStart = tempRoute.TotalElapsedSeconds;
                m_currClose.startTime = tempRoute.StartTime;
                m_currClose.stoppedTime = stoppedTime;

                closestList.Add(m_currClose);
            }

            if (difference > m_currentPointDiffFactor * m_maxDistanceError)
            {
                //Hysteresis, to make sure that the track really leaves the radius
                m_currClose.activity = null;
            }
        }
    }
}